#include "solve.h"
#include "matrix.h"

Matrix householder(Matrix g, Matrix &v, double &b)     //householder变换，返回值为对传入矩阵进行一次变换后的结果，传入参数除了要进行变换的矩阵g，还需要一个空向量v用来接收表征所用householder变换的向量v
{
    Matrix a = g.cut(1, g.row, 1, 1);
    Matrix c = a* (1 / a.norm0());
    double m = vnorm(c) * vnorm(c) - c.datap[0] * c.datap[0];
    v = c;
    double n = sqrt(c.datap[0] * c.datap[0] + m);
    if (c.datap[0]<0)
    {
        v.datap[0] = c.datap[0] - n;
    }
    else
    {
        v.datap[0] = -m/(c.datap[0] + n);
    }
    b = 2 * v.datap[0] * v.datap[0] / (m + v.datap[0] * v.datap[0]);
    v = v * (1 / v.datap[0]);
    Matrix i = trans(g) * v * b;
    Matrix d = g - v * trans(i);
    return d;
}

Matrix QR(Matrix a)     //QR分解，返回的矩阵上三角部分存储R，下三角部分存储每一次householder变换对应的v
{
    Matrix b = a;
    for (int i = 1; i < b.col+1; i++)
    {
        Matrix c = b.cut(i, b.row, i, b.col);
        Matrix v(b.row - i + 1, 1, 0);
        double e;
        Matrix d = householder(c,v,e);
        for (int j = i; j < b.row+1; j++)
        {
            for (int k = i; k < b.col+1; k++)
            {
                b.datap[(j - 1) * b.col + k - 1] = d.datap[(j-i) * d.col + k-i];
            }
        }
        for (int j = i + 1; j < b.row + 1; j++)
        {
            b.datap[(j - 1) * b.col + i - 1] = v.datap[j - i];
        }
    }
    return b;
}

Matrix QR_Q(Matrix a)     //QR分解，返回Q
{
    Matrix b = a;
    Matrix Q = ones(a.row);
    for (int i = 1; i < b.col+1; i++)
    {
        Matrix c = b.cut(i, b.row, i, b.col);
        Matrix v(b.row - i + 1, 1, 0);
        double e;
        Matrix d = householder(c,v,e);
        Matrix H = ones(v.row) - v * trans(v) * e;
        Matrix IH = ones(a.row);
        for (int j = i; j < b.row+1; j++)
        {
            for (int k = i; k < b.col+1; k++)
            {
                b.datap[(j - 1) * b.col + k - 1] = d.datap[(j-i) * d.col + k-i];
            }
        }
        for (int j = i + 1; j < b.row + 1; j++)
        {
            b.datap[(j - 1) * b.col + i - 1] = v.datap[j - i];
        }
        for (int j = i; j < b.row+1; j++)
        {
            for (int k = i; k < b.row+1; k++)
            {
                IH.datap[(j - 1) * IH.col + k - 1] = H.datap[(j-i) * H.col + k-i];
            }
        }
        Q = Q * IH;
    }
    return Q;
}

/*int main()
{
    double p[9] = {1, 3, 5, 2, 4, 6, 7, 9, 8};
    Matrix g(3, 3, p);
    Matrix v(g.row, 1, 0);
    Matrix c = householder(g,v);
    c.show();
    Matrix h = QR(g);
    h.show();
}*/